#include "Includes.h"

////////////////////

#define pause_val 0
void pause()
{
#if(pause_val==1)
	{
		while(!vexRT[Btn8U])
			wait1Msec(50);
	}
#endif
}
/////////////////////////////////////////
void AutoRoute_Section()
{

#define turning_max_to_src_speed 125
#define turning_max_to_dst_speed 125

#define rising_min_speed 					35
#define stick_to_limit_speed 			30
#define keep_lowest_speed 				-50
#define keep_inserting_speed 			-30

#define go_to_src_time 1000
#define go_to_dst_time 1100

#define Out_Of_Src_One_Key_time 1050
	writeDebugStream("autonomous\n");

	GBL_hgt_Lowest=SensorValue[Angle_Height];

	ClearTimer(T3);

	Claw_Statue_Func(Claw_Close);
	Turning_Arm_Encoder_TASK_FUN(dst_deg,turning_max_to_dst_speed,go_to_dst_time,T2,0);
	wait_sem_turn_arm();
	while(1);

	//////////////////////////////////////:1
	//// rise  && 	go to src 1
	Rise_Updown_AngleSor_TASK_FUN( GBL_hgt_Lowest+hgt_start_turn_arm+100,125,rising_min_speed,1050,T1,1); //rise

	StartTask(wait_to_start_turn_arm);//wait to turn arm
	wait_task(wait_to_start_turn_arm);

	wait_sem_rise_updown();
	Rise_Updown_Height_Lock_Fun(Height_Hold_Speed);//keep higher than lowest

	Turning_Arm_Encoder_TASK_FUN(src_deg,turning_max_to_src_speed,1000,T2,0);

	StartTask(wait_to_start_down);//wait to down
	wait_task(wait_to_start_down);

	Rise_Updown_AngleSor_TASK_FUN( GBL_hgt_Lowest+hgt_1_section_at_src,80,20,1050,T1,0); //go to hgt_1_section_at_src
	wait_sem_rise_updown_and_turn_arm();
	Rise_Updown_Height_Lock_Fun(Height_Hold_Speed);//keep higher than Lowest
	pause();
	wait1Msec(50);//wait to stable arm
	//take out 1
	Claw_Statue_Func(Claw_Close);
	Section_Out_Of_Src_One_Key(GBL_hgt_Lowest+hgt_section_out_src_one_key+20,125,Out_Of_Src_One_Key_time,T1);
	Rise_UpDown_Func(Height_Hold_Speed);//hold height
	pause();

	//go to dst 1
	Turning_Arm_Encoder_TASK_FUN(dst_deg,turning_max_to_dst_speed,go_to_dst_time,T2,1);
	wait_sem_turn_arm();
	//Turning_Arm_Func(-FIELD*stick_to_limit_speed); //stick to psychical limit
	StartTask(Down_Lowest_Get_Section_task);
	wait_lowest(); //1 section lowest putdown
	Rise_Updown_Height_Lock_Fun(keep_lowest_speed);//keep lower than Lowest

	pause();
	wait1Msec(200);
	Claw_Statue_Func(Claw_Open);
	pause();


	////////////////////////////////////////////////:2
	//go to src 2
	StartTask(Down_Lowest_Get_Section_task);
	Turning_Arm_Encoder_TASK_FUN(src_deg,turning_max_to_src_speed,go_to_src_time,T1,1);
	wait_two_task(Down_Lowest_Get_Section_task,Turning_Arm_Encoder_task);
	Rise_Updown_Height_Lock_Fun(keep_lowest_speed);//keep lower than Lowest
	pause();

	//take out 2
	Claw_Statue_Func(Claw_Close);
	Section_Out_Of_Src_One_Key(GBL_hgt_Lowest+hgt_section_out_src,125,Out_Of_Src_One_Key_time,T1);
	pause();

	//go to dst 2
	Rise_Updown_AngleSor_TASK_FUN(GBL_hgt_Lowest+hgt_2_section_to_dst-100,110,rising_min_speed,1000,T1,1); //rise
	Turning_Arm_Encoder_TASK_FUN(dst_deg,turning_max_to_dst_speed,go_to_dst_time,T2,1);
	wait_sem_rise_updown_and_turn_arm();
	Rise_Updown_Height_Lock_Fun(Height_Hold_Speed);


	//Turning_Arm_Func(-FIELD*stick_to_limit_speed); //stick to psychical limit
	////

	pause();
	wait1Msec(200);
	//lower to insert 2
	//Rise_Updown_AngleSor_TASK_FUN(GBL_hgt_Lowest+hgt_2_section_to_dst-hgt_section_insert,115,rising_min_speed,500,T1,0);
	Rise_Updown_AngleSor_TASK_FUN(SensorValue[Angle_Height]-hgt_section_insert,115,rising_min_speed,500,T1,0);

	wait_insert();
	Rise_UpDown_Func(keep_inserting_speed);
	wait1Msec(250);
	Claw_Statue_Func(Claw_Open);

	while(1)
		wait10Msec(20);
	////////////////////////////////////////////////:3
	//go to src 3
	StartTask(Down_Lowest_Get_Section_task);
	Turning_Arm_Encoder_TASK_FUN(src_deg,turning_max_to_src_speed,go_to_src_time,T1,1);
	wait_two_task(Down_Lowest_Get_Section_task,Turning_Arm_Encoder_task);
	Rise_Updown_Height_Lock_Fun(keep_lowest_speed);//keep lower than Lowest
	////take out 3
	pause();
	Claw_Statue_Func(Claw_Close);
	Section_Out_Of_Src_One_Key(GBL_hgt_Lowest+hgt_section_out_src,125,Out_Of_Src_One_Key_time,T1);
	pause();

	//go to dst 3
	Rise_Updown_AngleSor_TASK_FUN(GBL_hgt_Lowest+hgt_3_section_to_dst,100,rising_min_speed,1300,T1,1); //rise
	Turning_Arm_Encoder_TASK_FUN(dst_deg,turning_max_to_dst_speed,go_to_dst_time+200,T2,0);
	wait_sem_rise_updown_and_turn_arm();
	Rise_Updown_Height_Lock_Fun(Height_Hold_Speed);

	//Turning_Arm_Func(-FIELD*stick_to_limit_speed); //stick to psychical limit

	pause();


	wait1Msec(500);	// wait to stablize
	//lower to insert 3
	//	Rise_Updown_AngleSor_TASK_FUN(GBL_hgt_Lowest+hgt_3_section_to_dst-hgt_section_insert,100,rising_min_speed,500,T1,0);
	Rise_Updown_AngleSor_TASK_FUN(SensorValue[Angle_Height]-hgt_section_insert,115,rising_min_speed,500,T1,0);
	wait_insert();
	Rise_UpDown_Func(keep_inserting_speed);
	wait1Msec(250);	// wait to stablize
	Claw_Statue_Func(Claw_Open);


	////////////////////////////////////////////////:4
	//go to src 4
	StartTask(Down_Lowest_Get_Section_task);
	Turning_Arm_Encoder_TASK_FUN(src_deg,turning_max_to_src_speed,go_to_src_time,T1,1);
	wait_two_task(Down_Lowest_Get_Section_task,Turning_Arm_Encoder_task);
	Rise_Updown_Height_Lock_Fun(keep_lowest_speed);//keep lower than Lowest

	writeDebugStream("auto_all_time: %d\n",time1[T3]);
	//////////////
}
